#include <ros/ros.h>
#include <ros/package.h>
#include <std_srvs/Trigger.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/registration/ndt.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <string>
#include <vector>
#include <algorithm>
#include <fstream>
#include <boost/filesystem.hpp>

namespace fs = boost::filesystem;

class MapEvaluator {
public:
    MapEvaluator() : nh_(), private_nh_("~") {
        // 获取参数
        private_nh_.param<std::string>("maps_dir", maps_dir_, ros::package::getPath("auto_navigation") + "/maps");
        private_nh_.param<std::string>("best_map_path", best_map_path_, ros::package::getPath("auto_navigation") + "/maps/best_map.pcd");
        private_nh_.param<double>("voxel_leaf_size", voxel_leaf_size_, 0.2);
        private_nh_.param<int>("sor_mean_k", sor_mean_k_, 50);
        private_nh_.param<double>("sor_std_dev", sor_std_dev_, 1.0);
        private_nh_.param<bool>("auto_evaluate", auto_evaluate_, false);
        private_nh_.param<int>("evaluation_interval", evaluation_interval_, 30); // 30秒

        // 创建服务
        evaluate_service_ = nh_.advertiseService("evaluate_maps", &MapEvaluator::evaluateMapsCallback, this);
        
        // 创建发布器
        best_map_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("best_map", 1, true);
        
        // 如果设置了自动评估，启动定时器
        if (auto_evaluate_) {
            evaluation_timer_ = nh_.createTimer(
                ros::Duration(evaluation_interval_), 
                &MapEvaluator::evaluationTimerCallback, 
                this
            );
        }
        
        ROS_INFO("Map Evaluator initialized. Maps directory: %s", maps_dir_.c_str());
    }
    
    ~MapEvaluator() {
        ROS_INFO("Map Evaluator shutting down");
    }

private:
    // ROS相关
    ros::NodeHandle nh_;
    ros::NodeHandle private_nh_;
    ros::ServiceServer evaluate_service_;
    ros::Publisher best_map_pub_;
    ros::Timer evaluation_timer_;
    
    // 参数
    std::string maps_dir_;
    std::string best_map_path_;
    double voxel_leaf_size_;
    int sor_mean_k_;
    double sor_std_dev_;
    bool auto_evaluate_;
    int evaluation_interval_;
    
    // 评估地图的回调函数
    bool evaluateMapsCallback(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
        ROS_INFO("Evaluating maps in directory: %s", maps_dir_.c_str());
        
        // 获取目录中的所有PCD文件
        std::vector<std::string> map_files = findPCDFiles(maps_dir_);
        
        if (map_files.empty()) {
            ROS_WARN("No PCD files found in directory: %s", maps_dir_.c_str());
            res.success = false;
            res.message = "No PCD files found";
            return true;
        }
        
        ROS_INFO("Found %zu PCD files", map_files.size());
        
        // 评估所有地图
        std::string best_map_file;
        double best_score = std::numeric_limits<double>::max();
        
        for (const auto& map_file : map_files) {
            double score = evaluateMap(map_file);
            ROS_INFO("Map: %s, Score: %.4f", map_file.c_str(), score);
            
            if (score < best_score) {
                best_score = score;
                best_map_file = map_file;
            }
        }
        
        if (!best_map_file.empty()) {
            ROS_INFO("Best map: %s with score: %.4f", best_map_file.c_str(), best_score);
            
            // 处理并保存最佳地图
            if (processBestMap(best_map_file)) {
                res.success = true;
                res.message = "Best map processed and saved to: " + best_map_path_;
                
                // 发布最佳地图
                publishBestMap();
            } else {
                res.success = false;
                res.message = "Failed to process best map";
            }
        } else {
            res.success = false;
            res.message = "Failed to find best map";
        }
        
        return true;
    }
    
    // 定时评估的回调函数
    void evaluationTimerCallback(const ros::TimerEvent& event) {
        ROS_INFO("Auto evaluation triggered");
        std_srvs::Trigger::Request req;
        std_srvs::Trigger::Response res;
        evaluateMapsCallback(req, res);
    }
    
    // 查找目录中的所有PCD文件
    std::vector<std::string> findPCDFiles(const std::string& directory) {
        std::vector<std::string> files;
        
        if (!fs::exists(directory) || !fs::is_directory(directory)) {
            ROS_ERROR("Directory does not exist: %s", directory.c_str());
            return files;
        }
        
        for (fs::directory_iterator it(directory); it != fs::directory_iterator(); ++it) {
            if (fs::is_regular_file(it->status()) && it->path().extension() == ".pcd") {
                files.push_back(it->path().string());
            }
        }
        
        return files;
    }
    
    // 评估单个地图的质量
    double evaluateMap(const std::string& map_file) {
        // 加载点云
        pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
        if (pcl::io::loadPCDFile<pcl::PointXYZI>(map_file, *cloud) == -1) {
            ROS_ERROR("Failed to load PCD file: %s", map_file.c_str());
            return std::numeric_limits<double>::max();
        }
        
        // 评估标准：
        // 1. 点云密度 - 点数/体积
        // 2. 点云分布均匀性 - 标准差
        // 3. 点云完整性 - 覆盖范围
        
        // 计算点云体积
        double min_x = std::numeric_limits<double>::max();
        double min_y = std::numeric_limits<double>::max();
        double min_z = std::numeric_limits<double>::max();
        double max_x = -std::numeric_limits<double>::max();
        double max_y = -std::numeric_limits<double>::max();
        double max_z = -std::numeric_limits<double>::max();
        
        for (const auto& point : cloud->points) {
            min_x = std::min(min_x, static_cast<double>(point.x));
            min_y = std::min(min_y, static_cast<double>(point.y));
            min_z = std::min(min_z, static_cast<double>(point.z));
            max_x = std::max(max_x, static_cast<double>(point.x));
            max_y = std::max(max_y, static_cast<double>(point.y));
            max_z = std::max(max_z, static_cast<double>(point.z));
        }
        
        double volume = (max_x - min_x) * (max_y - min_y) * (max_z - min_z);
        if (volume <= 0) {
            ROS_WARN("Invalid volume for map: %s", map_file.c_str());
            return std::numeric_limits<double>::max();
        }
        
        // 计算点云密度
        double density = cloud->size() / volume;
        
        // 计算点云分布均匀性 (使用x, y坐标的标准差)
        double mean_x = 0, mean_y = 0;
        for (const auto& point : cloud->points) {
            mean_x += point.x;
            mean_y += point.y;
        }
        mean_x /= cloud->size();
        mean_y /= cloud->size();
        
        double variance_x = 0, variance_y = 0;
        for (const auto& point : cloud->points) {
            variance_x += (point.x - mean_x) * (point.x - mean_x);
            variance_y += (point.y - mean_y) * (point.y - mean_y);
        }
        variance_x /= cloud->size();
        variance_y /= cloud->size();
        
        double std_dev = std::sqrt(variance_x + variance_y);
        
        // 计算覆盖范围
        double coverage = (max_x - min_x) * (max_y - min_y);
        
        // 综合评分 (较低的分数表示更好的地图)
        // 我们希望高密度、均匀分布和大覆盖范围
        double score = 1.0 / (density * coverage) + std_dev;
        
        return score;
    }
    
    // 处理并保存最佳地图
    bool processBestMap(const std::string& map_file) {
        // 加载点云
        pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
        if (pcl::io::loadPCDFile<pcl::PointXYZI>(map_file, *cloud) == -1) {
            ROS_ERROR("Failed to load PCD file: %s", map_file.c_str());
            return false;
        }
        
        // 体素滤波降采样
        pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZI>);
        pcl::VoxelGrid<pcl::PointXYZI> voxel_filter;
        voxel_filter.setInputCloud(cloud);
        voxel_filter.setLeafSize(voxel_leaf_size_, voxel_leaf_size_, voxel_leaf_size_);
        voxel_filter.filter(*cloud_filtered);
        
        // 统计离群点滤波
        pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_filtered_sor(new pcl::PointCloud<pcl::PointXYZI>);
        pcl::StatisticalOutlierRemoval<pcl::PointXYZI> sor;
        sor.setInputCloud(cloud_filtered);
        sor.setMeanK(sor_mean_k_);
        sor.setStddevMulThresh(sor_std_dev_);
        sor.filter(*cloud_filtered_sor);
        
        // 保存处理后的最佳地图
        if (pcl::io::savePCDFileBinary(best_map_path_, *cloud_filtered_sor) == -1) {
            ROS_ERROR("Failed to save best map to: %s", best_map_path_.c_str());
            return false;
        }
        
        ROS_INFO("Best map saved to: %s", best_map_path_.c_str());
        return true;
    }
    
    // 发布最佳地图
    void publishBestMap() {
        // 加载最佳地图
        pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
        if (pcl::io::loadPCDFile<pcl::PointXYZI>(best_map_path_, *cloud) == -1) {
            ROS_ERROR("Failed to load best map for publishing: %s", best_map_path_.c_str());
            return;
        }
        
        // 转换为ROS消息并发布
        sensor_msgs::PointCloud2 cloud_msg;
        pcl::toROSMsg(*cloud, cloud_msg);
        cloud_msg.header.frame_id = "map";
        cloud_msg.header.stamp = ros::Time::now();
        
        best_map_pub_.publish(cloud_msg);
        ROS_INFO("Published best map");
    }
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "map_evaluator");
    
    MapEvaluator evaluator;
    
    ros::spin();
    
    return 0;
} 